
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_ekf_param.c
  * @author     baiyang
  * @date       2022-8-27
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
/* define parameters */
static param_t var_info[] = {
    PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.5e-2f), ///< IMU angular rate noise used for covariance prediction (rad/sec)
    PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 3.5e-1f), ///< IMU acceleration noise use for covariance prediction (m/sec**2)

    PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 1.0e-3f), ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
    PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 1.0e-2f), ///< process noise for IMU accelerometer bias prediction (m/sec**3)
    PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f), ///< process noise for earth magnetic field prediction (Gauss/sec)
    PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f), ///< process noise for body magnetic field prediction (Gauss/sec)
    PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f),  ///< process noise for wind velocity prediction (m/sec**2)

    PARAM_DEFINE_FLOAT(EKF2_GND_EFF_DZ, 5.0f),  ///< barometric deadzone range for negative innovations (m)
    PARAM_DEFINE_FLOAT(EKF2_GND_MAX_HGT, 0.5f), ///< maximum height above the ground level for expected negative baro innovations (m)

    // control of magnetometer fusion
    PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3.0e-1f), ///< measurement noise used for simple heading fusion (rad)
    PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f),  ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
    PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f),     ///< measurement noise used for airspeed fusion (m/sec)
    PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f),    ///< synthetic sideslip noise (rad)

    PARAM_DEFINE_FLOAT(EKF2_NOAID_NOISE, 10.0f), ///< observation noise for non-aiding position fusion (m)
    PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 2.0f),   ///< observation noise for barometric height fusion (m)

    PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0.0f), ///< magnetic declination (degrees)

    /**
     * Integer bitmask controlling handling of magnetic declination.
     *
     * Set bits in the following positions to enable functions.
     * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value.
     * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms.
     * 2 : Set to true to always use the declination as an observation when 3-axis magnetometer fusion is being used.
     *
     * @group EKF2
     * @min 0
     * @max 7
     * @bit 0 use geo_lookup declination
     * @bit 1 save EKF2_MAG_DECL on disarm
     * @bit 2 use declination as an observation
     * @reboot_required true
     */
    PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7),

    /**
     * Type of magnetometer fusion
     *
     * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector. The fuson of magnetomer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field.
     * If set to 'Automatic' magnetic heading fusion is used when on-ground and 3-axis magnetic field fusion in-flight with fallback to magnetic heading fusion if there is insufficient motion to make yaw or magnetic field states observable.
     * If set to 'Magnetic heading' magnetic heading fusion is used at all times
     * If set to '3-axis' 3-axis field fusion is used at all times.
     * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
     * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
     * If set to 'None' the magnetometer will not be used under any circumstance. If no external source of yaw is available, it is possible to use post-takeoff horizontal movement combined with GPS velocity measurements to align the yaw angle with the timer required (depending on the amount of movement and GPS data quality). Other external sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
     * @group EKF2
     * @value 0 Automatic
     * @value 1 Magnetic heading
     * @value 2 3-axis
     * @value 3 VTOL custom
     * @value 4 MC custom
     * @value 5 None
     * @reboot_required true
     */
    PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0),

    /**
     * Integer bitmask controlling GPS checks.
     *
     * Set bits to 1 to enable checks. Checks enabled by the following bit positions
     * 0 : Minimum required sat count set by EKF2_REQ_NSATS
     * 1 : Maximum allowed PDOP set by EKF2_REQ_PDOP
     * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH
     * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV
     * 4 : Maximum allowed speed error set by EKF2_REQ_SACC
     * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary.
     * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check will only run when the vehicle is on ground and stationary.
     * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary.
     * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT
     *
     * @group EKF2
     * @min 0
     * @max 511
     * @bit 0 Min sat count (EKF2_REQ_NSATS)
     * @bit 1 Max PDOP (EKF2_REQ_PDOP)
     * @bit 2 Max horizontal position error (EKF2_REQ_EPH)
     * @bit 3 Max vertical position error (EKF2_REQ_EPV)
     * @bit 4 Max speed error (EKF2_REQ_SACC)
     * @bit 5 Max horizontal position rate (EKF2_REQ_HDRIFT)
     * @bit 6 Max vertical position rate (EKF2_REQ_VDRIFT)
     * @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT)
     * @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT)
     */
    PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 245),

    // measurement source control

    /**
     * Integer bitmask controlling data fusion and aiding methods.
     *
     * Set bits in the following positions to enable:
     * 0 : Set to true to use GPS data if available
     * 1 : Set to true to use optical flow data if available
     * 2 : Set to true to inhibit IMU delta velocity bias estimation
     * 3 : Set to true to enable vision position fusion
     * 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
     * 5 : Set to true to enable multi-rotor drag specific force fusion
     * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
     * 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
     *
     * @group EKF2
     * @min 0
     * @max 511
     * @bit 0 use GPS
     * @bit 1 use optical flow
     * @bit 2 inhibit IMU bias estimation
     * @bit 3 vision position fusion
     * @bit 4 vision yaw fusion
     * @bit 5 multi-rotor drag fusion
     * @bit 6 rotate external vision
     * @bit 7 GPS yaw fusion
     * @bit 8 vision velocity fusion
     * @reboot_required true
     */
    PARAM_DEFINE_INT32(EKF2_AID_MASK, 1),

    /**
     * Determines the primary source of height data used by the EKF.
     *
     * The range sensor option should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level.
     *
     * @group EKF2
     * @value 0 Barometric pressure
     * @value 1 GPS
     * @value 2 Range sensor
     * @value 3 Vision
     * @reboot_required true
     */
    PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0),

    /**
     * Integer bitmask controlling fusion sources of the terrain estimator
     *
     * Set bits in the following positions to enable:
     * 0 : Set to true to use range finder data if available
     * 1 : Set to true to use optical flow data if available
     *
     * @group EKF2
     * @min 0
     * @max 3
     * @bit 0 use range finder
     * @bit 1 use optical flow
     */
    PARAM_DEFINE_INT32(EKF2_TERR_MASK, 3),

    // range finder fusion
    PARAM_DEFINE_FLOAT(EKF2_RNG_NOISE, 0.1f), ///< observation noise for range finder measurements (m)
    PARAM_DEFINE_INT32(EKF2_RNG_AID, 0),      ///< enables use of a range finder even if primary height source is not range finder

    // vision estimate fusion

    /**
     * Whether to set the external vision observation noise from the parameter or from vision message
     *
     * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound.
     *
     * @boolean
     * @group EKF2
     */
    PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0),
    PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.1f),                                            ///< default position observation noise for exernal vision measurements (m)
    PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f),                                            ///< default velocity observation noise for exernal vision measurements (m/s)
    PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f),                                           ///< default angular observation noise for exernal vision measurements (rad)

    PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f),                                            ///< Gate size for vision velocity estimate fusion
    PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f),                                           ///< Gate size for vision position fusion

    // optical flow fusion
    PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f), ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
    PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f),  ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)

    // control of airspeed and sideslip fusion
    PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 2.0f), ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
    PARAM_DEFINE_INT32(EKF2_FUSE_BETA, 0),   ///< Controls synthetic sideslip fusion, 0 disables, 1 enables

    // Multi-rotor drag specific force fusion
    PARAM_DEFINE_FLOAT(EKF2_DRAG_NOISE, 2.5f), ///< observation noise variance for drag specific force measurements (m/sec**2)**2

    PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0), ///< Mag field strength check

    PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0), ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.

    PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f) ///< Required GPS health time
};
PARAM_GROUP_DEFINE(EKF2, var_info);

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/

/*------------------------------------test------------------------------------*/


